/*
 * image.cpp
 *
 *  Created on: 2024年2月28日
 *      Author: 陈朝
 */
#include "image.h"

// 全局变量定义
int midline[UVC_HEIGHT] = {0};
int leftline[UVC_HEIGHT] = {0};
int rightline[UVC_HEIGHT] = {0};
uint8 binaryArray[UVC_HEIGHT][UVC_WIDTH] = {0};
uint8 original_image[UVC_HEIGHT][UVC_WIDTH] = {0};
uint8 bin_image[UVC_HEIGHT][UVC_WIDTH] = {0};


// 八邻域相关变量
uint16 points_l[USE_NUM][2] = {0}; // 左线点
uint16 points_r[USE_NUM][2] = {0}; // 右线点
uint16 dir_r[USE_NUM] = {0};       // 右边生长方向
uint16 dir_l[USE_NUM] = {0};       // 左边生长方向
uint16 data_stastics_l = 0;        // 左边找到点的个数
uint16 data_stastics_r = 0;        // 右边找到点的个数
uint16 l_border[UVC_HEIGHT];       // 左线数组
uint16 r_border[UVC_HEIGHT];       // 右线数组
uint16 l_index[UVC_HEIGHT];        // 左边界索引数组
uint16 r_index[UVC_HEIGHT];        // 右边界索引数组
uint8 center_line[UVC_HEIGHT];     // 中线数组

// 状态机相关变量
TrackType current_track_type = NORMAL_TRACK;
StateType current_state = SEARCH_MODE;
uint8 lost_counter = 0;            // 丢线计数器
uint8 cross_detected = 0;          // 十字检测标志
int16 open_speed = 0;            // 开环速度

// 斑马线相关变量
uint8 zebra_detected = 0;          // 斑马线检测标志
uint16 change_point = 0;           // 变化点计数器
struct timeval last_zebra_time = {0}; // 上次斑马线检测时间
uint8 zebra_count = 0;       // 斑马线检测计数


// 十字处理变量
uint8 flag_cross = 0;              // 十字标志位
int cross_jud = 0;                 // 十字状态判断
int cross_jud_time = 0;            // 十字状态计时


// 绝对值函数
int my_abs(int value) {
    return value >= 0 ? value : -value;
}

// 获取当前时间
static uint64_t get_current_time_ms() {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (uint64_t)tv.tv_sec * 1000 + tv.tv_usec / 1000;
}

// 限制值在[a,b]范围内
int16 limit_a_b(int16 x, int a, int b) {
    if(x < a) x = a;
    if(x > b) x = b;
    return x;
}

// 限制x在[-y,y]范围内
int16 limit1(int16 x, int16 y) {
    if(x > y) return y;
    else if(x < -y) return -y;
    else return x;
}

// 大津法求阈值
short GetOSTU(uint8 tmImage[UVC_HEIGHT][UVC_WIDTH]) {
    int16 i, j;
    unsigned long Amount = 0;
    unsigned long PixelBack = 0;
    unsigned long PixelIntegralBack = 0;
    unsigned long PixelIntegral = 0;
    int32 PixelIntegralFore = 0;
    int32 PixelFore = 0;
    float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma;
    int16 MinValue, MaxValue;
    int16 Threshold = 0;
    uint8 HistoGram[256] = {0};

    // 统计灰度直方图
    for(j = 0; j < UVC_HEIGHT; j++) {
        for(i = 0; i < UVC_WIDTH; i++) {
            HistoGram[tmImage[j][i]]++;
        }
    }

    // 获取最小和最大灰度值
    for(MinValue = 0; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++);
    for(MaxValue = 255; MaxValue > MinValue && HistoGram[MinValue] == 0; MaxValue--);

    if(MaxValue == MinValue) return MaxValue;   // 图像中只有一个颜色
    if(MinValue + 1 == MaxValue) return MinValue; // 图像中只有二个颜色

    // 计算像素总数和灰度总值
    for(j = MinValue; j <= MaxValue; j++) {
        Amount += HistoGram[j];
        PixelIntegral += HistoGram[j] * j;
    }

    SigmaB = -1;
    for(j = MinValue; j < MaxValue; j++) {
        PixelBack += HistoGram[j];
        PixelFore = Amount - PixelBack;
        OmegaBack = (float)PixelBack / Amount;
        OmegaFore = (float)PixelFore / Amount;
        PixelIntegralBack += HistoGram[j] * j;
        PixelIntegralFore = PixelIntegral - PixelIntegralBack;
        MicroBack = (float)PixelIntegralBack / PixelBack;
        MicroFore = (float)PixelIntegralFore / PixelFore;
        Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore);
        
        if(Sigma > SigmaB) {
            SigmaB = Sigma;
            Threshold = j;
        }
    }
    return Threshold;
}

// 图像滤波去噪
void Bin_Image_Filter(uint8 Bin_Image[UVC_HEIGHT][UVC_WIDTH]) {
    int16 nr, nc; // 行,列  

    for(nr = 1; nr < UVC_HEIGHT - 1; nr++) {
        for(nc = 1; nc < UVC_WIDTH - 1; nc++) {
            if((Bin_Image[nr][nc] == 0) &&
               (Bin_Image[nr-1][nc] + Bin_Image[nr+1][nc] + Bin_Image[nr][nc+1] + Bin_Image[nr][nc-1] > 2)) {
                Bin_Image[nr][nc] = 1;
            } else if((Bin_Image[nr][nc] == 1) &&
                     (Bin_Image[nr-1][nc] + Bin_Image[nr+1][nc] + Bin_Image[nr][nc+1] + Bin_Image[nr][nc-1] < 2)) {
                Bin_Image[nr][nc] = 0;
            }
        }
    }
}

// 获取图像数据
void Get_image(uint8(*mt9v03x_image)[UVC_WIDTH]) {
    uint8 i, j, row = 0, line = 0;
    for(i = 0; i < UVC_HEIGHT; i++) {
        for(j = 0; j < UVC_WIDTH; j++) {
            original_image[row][line] = mt9v03x_image[i][j];
            line++;
        }
        line = 0;
        row++;
    }
}

// 大津法阈值计算
uint8 otsuThreshold(uint8 *image, uint16 col, uint16 row) {
    #define GrayScale 256
    uint16 X, Y;
    uint8* data = image;
    int HistGram[GrayScale] = {0};
    uint32 Amount = 0;
    uint32 PixelBack = 0;       //像素背面
    uint32 PixelIntegralBack = 0;   
    uint32 PixelIntegral = 0;
    int32 PixelIntegralFore = 0;
    int32 PixelFore = 0;
    double OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma;
    uint8 MinValue = 0, MaxValue = 0;
    uint8 Threshold = 0;

    // 统计灰度直方图
    for(Y = 0; Y < row; Y++) {
        for(X = 0; X < col; X++) {
            HistGram[(int)data[Y*col + X]]++;
        }
    }

    // 获取最小和最大灰度值
    for(MinValue = 0; MinValue < 256 && HistGram[MinValue] == 0; MinValue++);
    for(MaxValue = 255; MaxValue > MinValue && HistGram[MinValue] == 0; MaxValue--);

    if(MaxValue == MinValue) return MaxValue;
    if(MinValue + 1 == MaxValue) return MinValue;

    // 计算像素总数和灰度总值
    for(Y = MinValue; Y <= MaxValue; Y++) {
        Amount += HistGram[Y];
        PixelIntegral += HistGram[Y] * Y;
    }

    SigmaB = -1;
    for(Y = MinValue; Y < MaxValue; Y++) {
        PixelBack += HistGram[Y];
        PixelFore = Amount - PixelBack;
        OmegaBack = (double)PixelBack / Amount;
        OmegaFore = (double)PixelFore / Amount;
        PixelIntegralBack += HistGram[Y] * Y;
        PixelIntegralFore = PixelIntegral - PixelIntegralBack;
        MicroBack = (double)PixelIntegralBack / PixelBack;
        MicroFore = (double)PixelIntegralFore / PixelFore;
        Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore);
        
        if(Sigma > SigmaB) {
            SigmaB = Sigma;
            Threshold = (uint8)Y;
        }
    }
    return Threshold;
}

// 图像二值化
void turn_to_bin(void) {
    uint8 i, j;
    uint8 image_thereshold = otsuThreshold(original_image[0], UVC_WIDTH, UVC_HEIGHT);
    
    for(i = 0; i < UVC_HEIGHT; i++) {
        for(j = 0; j < UVC_WIDTH; j++) {
            bin_image[i][j] = (original_image[i][j] > image_thereshold) ? WHITE_PIXEL : BLACK_PIXEL;
        }
    }
}

// 寻找起点
uint8 start_point_l[2] = {0}; // 左边起点的x,y值
uint8 start_point_r[2] = {0}; // 右边起点的x,y值
uint8 get_start_point(uint8 start_row) {
    uint8 i = 0, l_found = 0, r_found = 0;
    
    // 清零
    start_point_l[0] = 0; // x
    start_point_l[1] = 0; // y
    start_point_r[0] = 0; // x
    start_point_r[1] = 0; // y

    // 从中间往左边找起点
    for(i = UVC_WIDTH / 2; i > BORDER_MIN; i--) {
        start_point_l[0] = i; // x
        start_point_l[1] = start_row; // y
        if(bin_image[start_row][i] == WHITE_PIXEL && bin_image[start_row][i-1] == BLACK_PIXEL) {
            l_found = 1;
            break;
        }
    }

    // 从中间往右边找起点
    for(i = UVC_WIDTH / 2; i < BORDER_MAX; i++) {
        start_point_r[0] = i; // x
        start_point_r[1] = start_row; // y
        if(bin_image[start_row][i] == WHITE_PIXEL && bin_image[start_row][i+1] == BLACK_PIXEL) {
            r_found = 1;
            break;
        }
    }

    return (l_found && r_found) ? 1 : 0;
}

// 八邻域搜索主函数
void search_l_r(uint16 break_flag, uint8(*image)[UVC_WIDTH], uint16 *l_stastic, uint16 *r_stastic, 
               uint8 l_start_x, uint8 l_start_y, uint8 r_start_x, uint8 r_start_y, uint8*hightest) {
    uint8 i, j;
    
    // 左边变量
    uint8 search_filds_l[8][2] = {0};
    uint8 index_l = 0;
    uint8 temp_l[8][2] = {0};
    uint8 center_point_l[2] = {0};
    uint16 l_data_statics = *l_stastic;
    
    // 左边搜索方向 (顺时针)
    static int8 seeds_l[8][2] = {
        {0, 1}, {-1, 1}, {-1, 0}, {-1, -1}, 
        {0, -1}, {1, -1}, {1, 0}, {1, 1}
    };
    
    // 右边变量
    uint8 search_filds_r[8][2] = {0};
    uint8 center_point_r[2] = {0};
    uint8 index_r = 0;
    uint8 temp_r[8][2] = {0};
    uint16 r_data_statics = *r_stastic;
    
    // 右边搜索方向 (逆时针)
    static int8 seeds_r[8][2] = {
        {0, 1}, {1, 1}, {1, 0}, {1, -1}, 
        {0, -1}, {-1, -1}, {-1, 0}, {-1, 1}
    };
    
    // 初始化中心点
    center_point_l[0] = l_start_x;
    center_point_l[1] = l_start_y;
    center_point_r[0] = r_start_x;
    center_point_r[1] = r_start_y;
    
    // 开始八邻域搜索
    while(break_flag--) {
        // 左边处理
        for(i = 0; i < 8; i++) {
            search_filds_l[i][0] = center_point_l[0] + seeds_l[i][0];
            search_filds_l[i][1] = center_point_l[1] + seeds_l[i][1];
        }
        
        points_l[l_data_statics][0] = center_point_l[0];
        points_l[l_data_statics][1] = center_point_l[1];
        l_data_statics++;
        
        // 右边处理
        for(i = 0; i < 8; i++) {
            search_filds_r[i][0] = center_point_r[0] + seeds_r[i][0];
            search_filds_r[i][1] = center_point_r[1] + seeds_r[i][1];
        }
        
        points_r[r_data_statics][0] = center_point_r[0];
        points_r[r_data_statics][1] = center_point_r[1];
        
        index_l = 0;
        for(i = 0; i < 8; i++) {
            temp_l[i][0] = 0;
            temp_l[i][1] = 0;
        }
        
        // 左边边界判断
        for(i = 0; i < 8; i++) {
            if(image[search_filds_l[i][1]][search_filds_l[i][0]] == BLACK_PIXEL &&
               image[search_filds_l[(i+1)&7][1]][search_filds_l[(i+1)&7][0]] == WHITE_PIXEL) {
                temp_l[index_l][0] = search_filds_l[i][0];
                temp_l[index_l][1] = search_filds_l[i][1];
                index_l++;
                dir_l[l_data_statics-1] = i;
            }
            
            if(index_l) {
                center_point_l[0] = temp_l[0][0];
                center_point_l[1] = temp_l[0][1];
                for(j = 0; j < index_l; j++) {
                    if(center_point_l[1] > temp_l[j][1]) {
                        center_point_l[0] = temp_l[j][0];
                        center_point_l[1] = temp_l[j][1];
                    }
                }
            }
        }
        
        // 检查终止条件
        if((points_r[r_data_statics][0] == points_r[r_data_statics-1][0] && 
           points_r[r_data_statics][0] == points_r[r_data_statics-2][0] &&
           points_r[r_data_statics][1] == points_r[r_data_statics-1][1] && 
           points_r[r_data_statics][1] == points_r[r_data_statics-2][1]) ||
           (points_l[l_data_statics-1][0] == points_l[l_data_statics-2][0] && 
            points_l[l_data_statics-1][0] == points_l[l_data_statics-3][0] &&
            points_l[l_data_statics-1][1] == points_l[l_data_statics-2][1] && 
            points_l[l_data_statics-1][1] == points_l[l_data_statics-3][1])) {
            break;
        }
        
        if(my_abs(points_r[r_data_statics][0] - points_l[l_data_statics-1][0]) < 2 &&
           my_abs(points_r[r_data_statics][1] - points_l[l_data_statics-1][1]) < 2) {
            *hightest = (points_r[r_data_statics][1] + points_l[l_data_statics-1][1]) >> 1;
            break;
        }
        
        if(points_r[r_data_statics][1] < points_l[l_data_statics-1][1]) {
            continue; // 如果左边比右边高了，左边等待右边
        }
        
        if(dir_l[l_data_statics-1] == 7 && 
           (points_r[r_data_statics][1] > points_l[l_data_statics-1][1])) {
            // 左边开始向下了，等待右边
            center_point_l[0] = points_l[l_data_statics-1][0];
            center_point_l[1] = points_l[l_data_statics-1][1];
            l_data_statics--;
        }
        
        r_data_statics++;
        
        index_r = 0;
        for(i = 0; i < 8; i++) {
            temp_r[i][0] = 0;
            temp_r[i][1] = 0;
        }
        
        // 右边边界判断
        for(i = 0; i < 8; i++) {
            if(image[search_filds_r[i][1]][search_filds_r[i][0]] == BLACK_PIXEL &&
               image[search_filds_r[(i+1)&7][1]][search_filds_r[(i+1)&7][0]] == WHITE_PIXEL) {
                temp_r[index_r][0] = search_filds_r[i][0];
                temp_r[index_r][1] = search_filds_r[i][1];
                index_r++;
                dir_r[r_data_statics-1] = i;
            }
            
            if(index_r) {
                center_point_r[0] = temp_r[0][0];
                center_point_r[1] = temp_r[0][1];
                for(j = 0; j < index_r; j++) {
                    if(center_point_r[1] > temp_r[j][1]) {
                        center_point_r[0] = temp_r[j][0];
                        center_point_r[1] = temp_r[j][1];
                    }
                }
            }
        }
    }
    
    *l_stastic = l_data_statics;
    *r_stastic = r_data_statics;
}

// 获取左边界线
void get_left(uint16 total_L) {
    uint8 i = 0;
    uint16 j = 0;
    uint8 h = 0;
    
    // 初始化左线数组
    for(i = 0; i < UVC_HEIGHT; i++) {
        l_border[i] = BORDER_MIN;
        l_index [i] = 0;
    }
    
    h = UVC_HEIGHT - 2;
    for(j = 0; j < total_L; j++) {
        if(points_l[j][1] == h) {
            l_border[h] = points_l[j][0] + 1;
            l_index [h] = j; 
        } else {
            continue; // 每行只取一个点
        }
        
        h--;
        if(h == 0) break; // 到最后一行退出
    }
}

// 获取右边界线
void get_right(uint16 total_R) {
    uint8 i = 0;
    uint16 j = 0;
    uint8 h = 0;
    
    // 初始化右线数组
    for(i = 0; i < UVC_HEIGHT; i++) {
        r_index[i] = 0;
        r_border[i] = BORDER_MAX;//右边线初始化放到最右边，左边线放到最左边，这样八邻域闭合区域外的中线就会在中间，不会干扰得到的数据
    }
    
    h = UVC_HEIGHT - 2;
    for(j = 0; j < total_R; j++) {
        if(points_r[j][1] == h) {
            r_border[h] = points_r[j][0] - 1;
            r_index[h] = j;
        } else {
            continue; // 每行只取一个点
        }
        
        h--;
        if(h == 0) break; // 到最后一行退出
    }
}
// 形态学滤波 (膨胀和腐蚀)
#define THRESHOLD_MAX 255*5
#define THRESHOLD_MIN 255*2
void image_filter(uint8(*bin_image)[UVC_WIDTH]) {
    uint16 i, j;
    uint32 num = 0;
    
    for(i = 1; i < UVC_HEIGHT - 1; i++) {
        for(j = 1; j < (UVC_WIDTH - 1); j++) {
            // 统计八个方向的像素值
            num = bin_image[i-1][j-1] + bin_image[i-1][j] + bin_image[i-1][j+1] +
                  bin_image[i][j-1] + bin_image[i][j+1] +
                  bin_image[i+1][j-1] + bin_image[i+1][j] + bin_image[i+1][j+1];
            
            if(num >= THRESHOLD_MAX && bin_image[i][j] == BLACK_PIXEL) {
                bin_image[i][j] = WHITE_PIXEL; // 膨胀
            }
            if(num <= THRESHOLD_MIN && bin_image[i][j] == WHITE_PIXEL) {
                bin_image[i][j] = BLACK_PIXEL; // 腐蚀
            }
        }
    }
}

// 绘制图像边框
void image_draw_rectan(uint8(*image)[UVC_WIDTH]) {
    uint8 i = 0;
    
    for(i = 0; i < UVC_HEIGHT; i++) {
        image[i][0] = BLACK_PIXEL;
        image[i][1] = BLACK_PIXEL;
        image[i][UVC_WIDTH-1] = BLACK_PIXEL;
        image[i][UVC_WIDTH-2] = BLACK_PIXEL;
    }
    
    for(i = 0; i < UVC_WIDTH; i++) {
        image[0][i] = BLACK_PIXEL;
        image[1][i] = BLACK_PIXEL;
    }
}

//十字补线处理函数
/** 
* @brief 最小二乘法
* @param uint8 begin             输入起点
* @param uint8 end               输入终点
* @param uint8 *border           输入需要计算斜率的边界首地址
* @return 返回斜率
*/
float Slope_Calculate(uint8 begin, uint8 end, uint16 *border)
{
    float xsum = 0, ysum = 0, xysum = 0, x2sum = 0;
    int16 i = 0;
    float result = 0;
    static float resultlast;

    for (i = begin; i < end; i++)
    {
        xsum += i;
        ysum += border[i];
        xysum += i * (border[i]);
        x2sum += i * i;
    }
    
    if ((end - begin)*x2sum - xsum * xsum) // 判断除数是否为零
    {
        result = ((end - begin)*xysum - xsum * ysum) / ((end - begin)*x2sum - xsum * xsum);
        resultlast = result;
    }
    else
    {
        result = resultlast;
    }
    return result;
}

/** 
* @brief 计算斜率截距
* @param uint8 start             输入起点
* @param uint8 end               输入终点
* @param uint8 *border           输入需要计算斜率的边界
* @param float *slope_rate       输入斜率地址
* @param float *intercept        输入截距地址
*/
void calculate_s_i(uint8 start, uint8 end, uint16 *border, float *slope_rate, float *intercept)
{
    uint16 i, num = 0;
    uint16 xsum = 0, ysum = 0;
    float y_average, x_average;

    num = 0;
    xsum = 0;
    ysum = 0;
    y_average = 0;
    x_average = 0;
    for (i = start; i < end; i++)
    {
        xsum += i;
        ysum += border[i];
        num++;
    }

    //计算各个平均数
    if (num)
    {
        x_average = (float)(xsum / num);
        y_average = (float)(ysum / num);
    }

    /*计算斜率*/
    *slope_rate = Slope_Calculate(start, end, border); //斜率
    *intercept = y_average - (*slope_rate)*x_average; //截距
}





/** 
* @brief 十字补线函数
* @param uint8(*image)[UVC_WIDTH] 输入二值图像
* @param uint8 *l_border         输入左边界首地址
* @param uint8 *r_border         输入右边界首地址
* @param uint16 total_num_l       输入左边循环总次数
* @param uint16 total_num_r      输入右边循环总次数
* @param uint16 *dir_l           输入左边生长方向首地址
* @param uint16 *dir_r           输入右边生长方向首地址
* @param uint16(*points_l)[2]    输入左边轮廓首地址
* @param uint16(*points_r)[2]    输入右边轮廓首地址
*/
 void cross_fill(uint8(*image)[UVC_WIDTH], uint16 *l_border, uint16 *r_border, 
               uint16 total_num_l, uint16 total_num_r,
               uint16 *dir_l, uint16 *dir_r, 
               uint16(*points_l)[2], uint16(*points_r)[2]) 
{
    // 0. 检查输入参数是否有效
    if (!image || !l_border || !r_border || !dir_l || !dir_r || !points_l || !points_r) {
        printf("Error: NULL pointer in cross_fill\n");
        return;
    }

    // 1. 确保有足够的数据点
    if (total_num_l < 10 || total_num_r < 10) {
        return;
    }

    uint8 break_num_l = 0;
    uint8 break_num_r = 0;
    float slope_l_rate = 0, intercept_l = 0;

    // 2. 查找左边界断点（确保 i + 7 < total_num_l）
    for (uint16 i = 1; i + 7 < total_num_l; i++) {
        if (dir_l[i - 1] == 4 && dir_l[i] == 4 && 
            dir_l[i + 3] == 6 && dir_l[i + 5] == 6 && 
            dir_l[i + 7] == 6) 
        {
            break_num_l = points_l[i][1];
            break;
        }
    }

    // 3. 查找右边界断点（确保 i + 7 < total_num_r）
    for (uint16 i = 1; i + 7 < total_num_r; i++) {
        if (dir_r[i - 1] == 4 && dir_r[i] == 4 && 
            dir_r[i + 3] == 6 && dir_r[i + 5] == 6 && 
            dir_r[i + 7] == 6) 
        {
            break_num_r = points_r[i][1];
            break;
        }
    }

    // 4. 检查是否找到断点，并且图像底部有有效数据
    if (!break_num_l || !break_num_r || 
        !image[UVC_HEIGHT - 1][4] || 
        !image[UVC_HEIGHT - 1][UVC_WIDTH - 4]) 
    {
        return;
    }

    // 5. 左边界补线（确保有足够的数据计算斜率）
    if (break_num_l > 15) {
        uint8 start = limit_a_b(break_num_l - 15, 0, UVC_HEIGHT);
        uint8 end = limit_a_b(break_num_l - 5, 0, UVC_HEIGHT);
        calculate_s_i(start, end, l_border, &slope_l_rate, &intercept_l);

        for (uint8 i = break_num_l - 5; i < UVC_HEIGHT - 1; i++) {
            l_border[i] = limit_a_b((int16)(slope_l_rate * i + intercept_l), BORDER_MIN, BORDER_MAX);
        }
    }

    // 6. 右边界补线（确保有足够的数据计算斜率）
    if (break_num_r > 15) {
        uint8 start = limit_a_b(break_num_r - 15, 0, UVC_HEIGHT);
        uint8 end = limit_a_b(break_num_r - 5, 0, UVC_HEIGHT);
        calculate_s_i(start, end, r_border, &slope_l_rate, &intercept_l);

        for (uint8 i = break_num_r - 5; i < UVC_HEIGHT - 1; i++) {
            r_border[i] = limit_a_b((int16)(slope_l_rate * i + intercept_l), BORDER_MIN, BORDER_MAX);
        }
    }
}



// 圆环相关变量
uint8_t cir_stage = 0;
uint8_t Cir_hengyi_Flag = 0;
uint8 left_right_flag = 0;
int16_t r_lose_num = 0;
int16_t l_lose_num = 0;
int16 length = 0;
uint8_t l_dir_sum[9] = {0};
uint8_t r_dir_sum[9] = {0};

// 添加圆环判断函数实现
bool iscircle(unsigned char *l, unsigned char *r, unsigned char(*p)[UVC_WIDTH]) {

    unsigned char h, w;
    unsigned char leftbreak = 0;
    bool circlefind = 0;
    bool ap = 0;
    
    for (h = UVC_HEIGHT - 10; h > 5; h--) {
        if (l[h] < l[h + 1] && l[h + 1] >= l[h + 10] && abs(l[h] - l[h + 1]) >= 5
            && l[h + 1]>5 && h < UVC_HEIGHT - 5 && h>20 && abs(l[h] - l[h + 1]) >= 10) {
   
            //printf("p %d,%d\n", p[h + 1][l[h + 1]],h+1); 
                if (p[h + 1][l[h + 1]] == 255 && p[h - 1][l[h + 1] + 5] == 255
                && p[h + 2][l[h + 1] - 5] == 0 && p[h + 2][l[h + 1] + 5] == 255
                && p[h - 1][l[h + 1] - 5] == 255 && p[h + 5][l[h + 1]] == 255) {

                    //printf("left break\n");
                leftbreak = h;
                break;
            }
        }
    }

    if (leftbreak) {
        ap = 1;
        for (h = leftbreak + 5; h > leftbreak - 15; h--) {
            if (r[h - 10] > r[h]) {
                //printf("right break\n");
                ap = 0;
                break;
            }
        }
    }
    // printf("ap=%d",ap);
    if (ap) {
        for (h = leftbreak - 15; h > 5; h--) {
            if (l[h] > l[h + 5] && l[h] > l[h - 5]) {
                circlefind = 1;
                // printf("right break\n");
                break;
            }
        }
    }

    // printf("状态一：%d\n",circlefind);
    return circlefind;
}

bool liscircle2(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, w;
    uint8 leftbreak = 0;
    uint8 op = 0;
    bool circlefind = 0;
    bool ap = 0;

    //printf("--- 开始寻找圆环 ---\n");
    
	for (h = UVC_HEIGHT - 5; h > 5; h--)
	{
		if (l[h + 1] > l[h] && abs(l[h + 1] - l[h]) > 5
			&&points_l[indexl[h + 1]][1] <= points_l[indexl[h + 1] + 3][1]
			&&points_l[indexl[h + 1]][1] < points_l[indexl[h + 1] + 7][1]
			&&points_l[indexl[h + 1] + 3][1] <= points_l[indexl[h + 1] + 7][1])        
		{
            leftbreak = h ;
            ap = 1;
            break;
        }
    }
    // printf("ap=%d,leftbreak=%d\n",ap,leftbreak);

	if (leftbreak)
	{
		for (w = leftbreak + 5; w > leftbreak - 15; w--)
		{
			if (r[w - 10] > r[w] && (r[w - 10] != BORDER_MAX || r[w] != BORDER_MAX))
			{
				ap = 0;
				break;
			}
		}
	}

	if (ap)
	{
		for (h = leftbreak - 15; h > 5; h--)
		{
			if (l[h] > l[h + 5] && l[h] > l[h - 5] && l[h - 5] != BORDER_MIN)
			{
				op = h;
				circlefind = 1;
				break;
			}
		}
	}
	if (circlefind)
	{
		float k = 0, b = 0;
		k = ((float)(l[leftbreak] - l[op]) / (float)(leftbreak - op));
		b = (float)(l[op] - op * k);
		for (unsigned char i = op; i < leftbreak + 2; i++)
		{
			l[i] = k * (i)+b;//y = kx+b
			l[i] = limit_a_b(l[i], BORDER_MIN, BORDER_MAX);//限幅
		}
	}
	return  circlefind;
}

bool lfixcircle2(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, w;
    uint8 leftbreak = 0;
    uint8 op = 0;
    bool circlefind = 0;
    bool ap = 0;
    
    for (h = UVC_HEIGHT - 5; h > UVC_HEIGHT >> 1; h--) {
        if (l[h + 1] > l[h] && abs(l[h + 1] - l[h]) > 5 &&
            points_l[indexl[h + 1]][1] <= points_l[indexl[h + 1] + 3][1] &&
            points_l[indexl[h + 1]][1] < points_l[indexl[h + 1] + 7][1] &&
            points_l[indexl[h + 1] + 3][1] <= points_l[indexl[h + 1] + 7][1]) {
            leftbreak = h + 3;
            break;
        }
    }

    if (leftbreak) {
        for (h = leftbreak - 15; h > 5; h--) {
            if (l[h] > l[h + 5] && l[h] > l[h - 5] && l[h - 5] != BORDER_MIN) {
                op = h;
                break;
            }
        }
    }
    
    if (op && leftbreak && op != leftbreak) {
        float k = 0, b = 0;
        k = ((float)(l[leftbreak] - l[op]) / (float)(leftbreak - op));
        b = (float)(l[op] - op * k);

        for (uint8 i = op; i < leftbreak + 2; i++) {
            l[i] = k * i + b;
            l[i] = limit_a_b(l[i], BORDER_MIN, BORDER_MAX);
        }
    }
    
    return circlefind;
}
bool liscircle3(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, w;
    uint8 op = 0;
    bool circlefind = 0;
    bool dp = 0;

    /*white block*/
    if (p[UVC_HEIGHT - 1][3] && p[UVC_HEIGHT - 3][3] && p[UVC_HEIGHT - 5][3]) {
        dp = 1;
    }

    if (dp) {
        for (h = UVC_HEIGHT - 15; h > 5; h--) {
            if (l[h] > l[h + 5] && l[h] > l[h - 5] && l[h - 5] != BORDER_MIN) {
                op = h;
                break;
            }
        }
    }

    if (dp && op) {
        circlefind = 1;
        float k = 0, b = 0;
        k = ((float)(l[UVC_HEIGHT - 1] - l[op]) / (float)(UVC_HEIGHT - 1 - op));
        b = (float)(l[op] - op * k);

        for (uint8 i = op; i < UVC_HEIGHT - 1; i++) {
            l[i] = k * i + b;
            l[i] = limit_a_b(l[i], BORDER_MIN, BORDER_MAX);
        }
    }
    
    // printf("clris3=%d",circlefind);

    return circlefind;
}
bool lfixcircle3(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, w;
    uint8 op = 0;
    bool circlefind = 0;
    bool dp = 0;

    for (h = UVC_HEIGHT - 15; h > 5; h--) {
        if (l[h] > l[h + 5] && l[h] > l[h - 5] && l[h - 5] != BORDER_MIN) {
            op = h;
            break;
        }
    }
    
    if (op) {
        float k = 0, b = 0;
        k = ((float)(l[UVC_HEIGHT - 1] - l[op]) / (float)(UVC_HEIGHT - 1 - op));
        b = (float)(l[op] - op * k);
        int temp = 0;
        
        for (uint8 i = op; i < UVC_HEIGHT - 1; i++) {
            temp = k * i + b;
            l[i] = limit_a_b(temp, BORDER_MIN, BORDER_MAX);
        }
    }
    
    return circlefind;
}
bool liscircle4(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, temph = 0;
    uint8 vp = 0;
    bool circlefind = 0;
    bool dp = 0;
    
    if (!p[UVC_HEIGHT - 5][5] && !p[UVC_HEIGHT - 3][3]) {
        dp = 1;
    }

    if (dp) {
        for (h = UVC_HEIGHT - 15; h > 5; h--) {
            if (l[h] > l[h + 1] && abs(l[h] - l[h + 1]) > 10) {
                temph = h;
                break;
            }
        }
    }

    if (temph) {
        for (int j = indexl[h]; j > 0; j--) {
            if (points_l[j][1] >= points_l[j + 3][1] &&
                points_l[j][1] > points_l[+5][1] &&
                points_l[j][1] >= points_l[-3][1] &&
                points_l[j][1] >= points_l[j - 5][1]) {
                vp = h;
                circlefind = 1;
                break;
            }
        }
    }
    
    if (vp) {
        float k = 0, b = 0;
        k = ((float)(r[UVC_HEIGHT - 2] - l[vp]) / (float)(UVC_HEIGHT - 2 - vp));
        b = (float)(l[vp] - vp * k);

        for (uint8 i = 0; i < UVC_HEIGHT - 1; i++) {
            r[i] = k * i + b;
            r[i] = limit_a_b(r[i], BORDER_MIN, BORDER_MAX);
        }
    }
    
    return circlefind;
}

bool lfixcircle4(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl)
{
    uint8 h, temph;
    uint8 op = 0;
    bool circlefind = 0;
    bool dp = 0;
	unsigned char vpx = 0, vpy = 0;
	for (h = UVC_HEIGHT - 15; h > 5; h--)
	{
		if (l[h] > l[h + 1] && abs(l[h] - l[h + 1]) > 30)
		{
			temph = h;
			break;
		}
	}


	if (temph)
	{
		for (int i = data_stastics_l - 10; i > indexl[h + 1]; i--)
		{

			if (points_l[i][1] >= points_l[i + 3][1]
				&&points_l[i][1] > points_l[i + 5][1]
				&&points_l[i][1] >= points_l[i - 3][1]
				&&points_l[i][1] >= points_l[i - 5][1]
				&&points_l[i][0] > points_l[i - 5][0]
				&&points_l[i][0] <= points_l[i + 5][0])
			{
				vpy = points_l[i][1];
				vpx = points_l[i][0];
				break;
			}
		}
	}

	if (vpy&&vpx)
	{
		float k = 0, b = 0;
		k = (float)(r[UVC_HEIGHT - 1] - vpx) / (float)(UVC_HEIGHT - 1 - vpy);
		b = (float)(vpx - (vpy * k));
		for (unsigned char i = 0; i < UVC_HEIGHT - 1; i++)
		{
			r[i] = k * (i)+b;//y = kx+b
			r[i] = limit_a_b(r[i], BORDER_MIN, BORDER_MAX);//限幅
		}
	}
	return  0;
}

bool liscircle5(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, temph = 0;
    uint8 vp = 0;
    bool circlefind = 0;
    bool dp = 0;
    uint8 dpx = 0, dpy = 0;

    if (p[UVC_HEIGHT - 4][UVC_WIDTH - 3] && p[UVC_HEIGHT - 6][UVC_WIDTH - 3] && p[UVC_HEIGHT - 8][UVC_WIDTH - 3] &&
        !p[UVC_HEIGHT - 4][3] && !p[UVC_HEIGHT - 4][5] && !p[UVC_HEIGHT - 2][5]) {
        dp = 1;
    }

    if (dp) {
        for (uint8 i = UVC_HEIGHT - 4; i > 10; i--) {
            if (r[i] < r[i + 3] && abs(r[i] - r[i + 3]) > 10) {
                temph = i;
                break;
            }
        }
    }

    if (temph) {
        for (int j = indexl[temph + 1]; j < data_stastics_r; j++) {
            if (points_r[j][1] >= points_r[j + 3][1] &&
                points_r[j][1] > points_r[j + 5][1] &&
                points_r[j][1] >= points_r[j - 3][1] &&
                points_r[j][1] >= points_r[j - 5][1] &&
                points_r[j][0] < points_r[j - 5][0] &&
                points_r[j][0] > points_r[j + 5][0]) {
                dpy = points_r[j][1];
                dpx = points_r[j][0];
                break;
            }
        }
    }

    if (dpx && dpy) {
        float k = 0, b = 0;
        k = ((float)r[UVC_HEIGHT - 1] - dpx) / (float)(UVC_HEIGHT - 1 - dpy);
        b = (float)(dpx - (dpy * k));

        for (uint8 i = dpy; i < UVC_HEIGHT - 1; i++) {
            float temp = k * i + b;
            r[i] = limit_a_b(temp, BORDER_MIN, BORDER_MAX);
        }
    }
    
    return dp;
}

bool liscircle6(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, w;
    uint8 vp = 0;
    bool circlefind = 0;
    bool dp = 0;
    
    if (!p[UVC_HEIGHT - 1][3] && !p[UVC_HEIGHT - 3][3] && 
        !p[UVC_HEIGHT - 1][UVC_WIDTH - 3] && !p[UVC_HEIGHT - 3][UVC_WIDTH - 3]) {
        dp = 1;
    }
    
    return dp;
}
bool lfixcircle6(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, w;
    uint8 vp = 0;
    bool circlefind = 0;
    bool dp = 0;
    
    for (uint8 i = UVC_HEIGHT - 9; i > 5; i--) {
        if (r[i] < r[i - 3] && abs(r[i] - r[i - 3]) > 10) {
            vp = i;
            circlefind = 1;
            break;
        }
    }

    if (vp) {
        float k = 0, b = 0;
        k = ((float)(r[vp + 9] - r[vp]) / (float)(9));
        b = (float)(r[vp] - k * vp);

        for (uint8 i = vp; i > 0; i--) {
            r[i] = k * i + b;
            l[i] = BORDER_MIN;
            r[i] = limit_a_b(r[i], BORDER_MIN, BORDER_MAX);
        }
    }
    
    return circlefind;
}

bool liscircle7(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, w;
    uint8 vp = 0;
    bool circlefind = 0;
    bool dp = 0;
    
    if (p[UVC_HEIGHT - 5][UVC_WIDTH - 3] && p[UVC_HEIGHT - 3][UVC_WIDTH - 3] &&
        p[UVC_HEIGHT - 7][UVC_WIDTH - 3]) {
        dp = 1;
    }
    
    return dp;
}

bool lfixcircle7(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h = 0, temph = 0;
    uint8 vpx = 0, vpy = 0;
    bool circlefind = 0;
    bool dp = 0;

    for (h = UVC_HEIGHT - 9; h > 0; h--) {
        if (l[h] > l[h + 1] && abs(l[h] - l[h + 1]) > 20) {
            temph = h;
            break;
        }
    }

    if (temph) {
        float k = 0, b = 0;
        k = ((float)(l[UVC_HEIGHT - 1] - l[temph]) / (float)(UVC_HEIGHT - 1 - temph));
        b = (float)(l[UVC_HEIGHT - 1] - k * (UVC_HEIGHT - 1));
        
        for (uint8 i = temph - 3; i < UVC_HEIGHT - 1; i++) {
            l[i] = k * i + b;
            l[i] = limit_a_b(l[i], BORDER_MIN, BORDER_MAX);
        }
    }
    
    return circlefind;
}


bool liscircle8(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, w;
    uint8 vp = 0;
    bool circlefind = 0;
    bool dp = 0;
    
    if (!p[UVC_HEIGHT - 5][UVC_WIDTH - 3] && !p[UVC_HEIGHT - 3][UVC_WIDTH - 3] &&
        !p[UVC_HEIGHT - 7][UVC_WIDTH - 3] &&
        p[UVC_HEIGHT - 5][3] && p[UVC_HEIGHT - 3][3] &&
        p[UVC_HEIGHT - 7][3]) {
        dp = 1;
    }
    
    return dp;
}

bool liscircle9(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexl) {
    uint8 h, w;
    uint8 vp = 0;
    bool circlefind = 0;
    bool dp = 0;
    
    if (!p[UVC_HEIGHT - 5][UVC_WIDTH - 3] && !p[UVC_HEIGHT - 3][UVC_WIDTH - 3] &&
        !p[UVC_HEIGHT - 7][UVC_WIDTH - 3] &&
        !p[UVC_HEIGHT - 5][3] && !p[UVC_HEIGHT - 3][3] &&
        !p[UVC_HEIGHT - 7][3]) {
        dp = 1;
    }
    
    return dp;
}

bool riscircle2(uint8 *l, uint8 *r, uint8(*p)[UVC_WIDTH], uint16 *indexr) {
    uint8 h, w;
    uint8 rightbreak = 0;
    bool circlefind = 0;
    bool ap = 0;
    
    for (h = UVC_HEIGHT - 5; h > 5; h--) {
        if (r[h + 1] < r[h] && abs(r[h + 1] - r[h]) > 5 &&
            points_r[indexr[h + 1]][1] <= points_r[indexr[h + 1] + 3][1] &&
            points_r[indexr[h + 1]][1] < points_r[indexr[h + 1] + 7][1] &&
            points_r[indexr[h + 1] + 3][1] <= points_r[indexr[h + 1] + 7][1]) {
            rightbreak = h;
            break;
        }
    }

    if (rightbreak) {
        ap = 1;
        for (w = rightbreak + 5; w > rightbreak - 15; w--) {
            if (l[w - 10] < l[w] && (l[w - 10] != BORDER_MIN || l[w] != BORDER_MIN)) {
                ap = 0;
                break;
            }
        }
    }

    if (ap) {
        for (h = rightbreak - 15; h > 5; h--) {
            if (r[h] < r[h + 5] && r[h] < r[h - 5] && r[h - 5] != BORDER_MAX) {
                circlefind = 1;
                break;
            }
        }
    }
    
    return circlefind;
}


void circletemp(unsigned char(*img)[UVC_WIDTH], unsigned char *l, unsigned char *r)
{
    printf("come in\n");  //deal

    static unsigned char statues = 0, circlrtype = 0; // left:1 right :2
    bool lflag = 0, rflag = 0;
    
    /*first find*/
    if (!statues) {
        lflag = liscircle2(l, r, img, l_index);
        rflag = riscircle2(l, r, img, r_index);
        if (lflag || rflag) {
            statues = 1;
            if (lflag)circlrtype = 1;
            if (rflag)circlrtype = 2;
            //  printf("sta=%d,cir=%d\n",statues,circlrtype);   //deal
        }
    }
    
   
    //  printf("sta=%d,cir=%d\n",statues,circlrtype);

    /*statues 2 function*/
    if (statues == 1 && circlrtype == 1) {
        bool is3find = 0;
        is3find = liscircle3(l, r, img, l_index);
        //lfixcircle2(l, r, img, l_index);
        if (is3find)statues = 2;
    }
    
    /*statues 3 function*/
    if (statues == 2 && circlrtype == 1) {
        bool is4find = 0;
        is4find = liscircle4(l, r, img, l_index);
        lfixcircle3(l, r, img, l_index);
        if (is4find)statues = 3;
    }
    
    /*statues 4 function*/
    if (statues == 3 && circlrtype == 1) {
        bool is5find = 0;
        is5find = liscircle5(l, r, img, r_index);
        lfixcircle4(l, r, img, l_index);
        if (is5find)statues = 4;
    }
    
    /*statues 5 function*/
    if (statues == 4 && circlrtype == 1) {
        bool is6find = 0;
        is6find = liscircle6(l, r, img, l_index);
        if (is6find)statues = 5;
    }
    
    /*statues 6 function*/
    if (statues == 5 && circlrtype == 1) {
        bool is7find = 0;
        is7find = liscircle7(l, r, img, l_index);
        lfixcircle6(l, r, img, r_index);
        if (is7find)statues = 6;
    }
    
    /*statues 6 function*/
    if (statues == 6 && circlrtype == 1) {
        bool is8find = 0;
        is8find = liscircle8(l, r, img, l_index);
        if (is8find)statues = 7;
    }
    
    /*statues 7 function*/
    if (statues == 7 && circlrtype == 1) {
        bool is9find = 0;
        is9find = liscircle9(l, r, img, l_index);
        lfixcircle7(l, r, img, r_index);
        if (is9find)statues = 0;
    }
}

// 添加圆环处理函数
void circle_track_process(void) {
    uint8 hightest = 0;
    uint8 l_border_u8[UVC_HEIGHT];
    uint8 r_border_u8[UVC_HEIGHT];
    
    for(int i = 0; i < UVC_HEIGHT; i++) {
        l_border_u8[i] = (uint8)l_border[i];
        r_border_u8[i] = (uint8)r_border[i];
    
    }
    
    circletemp(bin_image, l_border_u8, r_border_u8);
    
    // 更新边界线
    for(int i = 0; i < UVC_HEIGHT; i++) {
        l_border[i] = l_border_u8[i];
        r_border[i] = r_border_u8[i];
        midline[i] = (l_border[i] + r_border[i]) / 2;
    }
    
    // 检查是否离开圆环
    if(cir_stage == 0) {
        current_track_type = NORMAL_TRACK;
        current_state = SEARCH_MODE;
    }
}

/**
 * @brief 斑马线检测函数
 * @param row 要检测的行号
 */
// 斑马线检测函数
void detect_zebra_line(uint8 row) {
    change_point = 0;
    
    // 确保行号在有效范围内
    if(row < 2 || row >= UVC_HEIGHT) return;
    
    // 检查左右边界是否有效
    if(l_border[row] >= r_border[row] || 
       l_border[row] < BORDER_MIN || 
       r_border[row] > BORDER_MAX) {
        return;
    }
    
    // 扫描指定行及其上一行
    for(uint16 i = l_border[row]; i < r_border[row] - 3; i++) {
        // 检测黑白交替变化点
        if((bin_image[row][i] == BLACK_PIXEL && bin_image[row][i+1] == WHITE_PIXEL) ||   // 黑变白
           (bin_image[row][i] == WHITE_PIXEL && bin_image[row][i+1] == BLACK_PIXEL) ||   // 白变黑
           (bin_image[row-1][i] == BLACK_PIXEL && bin_image[row-1][i+1] == WHITE_PIXEL) || // 上一行黑变白
           (bin_image[row-1][i] == WHITE_PIXEL && bin_image[row-1][i+1] == BLACK_PIXEL)) { // 上一行白变黑
            change_point++;
        }
    }
    
    // 如果变化点超过阈值，判定为斑马线
    if(change_point > 15) {
        uint64_t current_time = get_current_time_ms();
        uint64_t last_time = (uint64_t)last_zebra_time.tv_sec * 1000 + last_zebra_time.tv_usec / 1000;
        
        // 如果是第一次检测到斑马线，或者距离上次检测时间超过最小间隔
        if(zebra_count == 0 || (current_time - last_time) > ZEBRA_MIN_INTERVAL) {
            zebra_count++;
            gettimeofday(&last_zebra_time, NULL); // 更新最后检测时间
            
            // 只有第二次检测到斑马线时才停车
            if(zebra_count == 2) {
                zebra_detected = 1;
                current_track_type = ZEBRA_TRACK;
            }
        }
    } else {
        zebra_detected = 0;
    }
}
// void detect_zebra_line(uint8 row) {
//     change_point = 0;
    
//     // 确保行号在有效范围内
//     if(row < 2 || row >= UVC_HEIGHT) return;
    
//     // 检查左右边界是否有效
//     if(l_border[row] >= r_border[row] || 
//        l_border[row] < BORDER_MIN || 
//        r_border[row] > BORDER_MAX) {
//         return;
//     }
    
//     // 扫描指定行及其上一行
//     for(uint16 i = l_border[row]; i < r_border[row] - 3; i++) {
//         // 检测黑白交替变化点
//         if((bin_image[row][i] == BLACK_PIXEL && bin_image[row][i+1] == WHITE_PIXEL) ||   // 黑变白
//            (bin_image[row][i] == WHITE_PIXEL && bin_image[row][i+1] == BLACK_PIXEL) ||   // 白变黑
//            (bin_image[row-1][i] == BLACK_PIXEL && bin_image[row-1][i+1] == WHITE_PIXEL) || // 上一行黑变白
//            (bin_image[row-1][i] == WHITE_PIXEL && bin_image[row-1][i+1] == BLACK_PIXEL)) { // 上一行白变黑
//             change_point++;
//         }
//     }
    
//     // 如果变化点超过阈值，判定为斑马线
//     if(change_point > 15) { // 阈值可调整如果斑马线检测太敏感（误检），增大这个值.如果斑马线检测不灵敏（漏检），减小这个值,通常为20-30.
//         zebra_detected = 1;
//         current_track_type = ZEBRA_TRACK;
//     } else {
//         zebra_detected = 0;
//     }
// }

// 状态机初始化
void state_machine_init(void) {
    open_speed=1800;   //电机开环
    current_track_type = NORMAL_TRACK;
    current_state = SEARCH_MODE;
    lost_counter = 0;
    cross_detected = 0;
    zebra_detected = 0;
    zebra_count = 0;        // 重置斑马线计数
    timerclear(&last_zebra_time); // 清零时间

}

// 检测赛道类型
void detect_track_type(void) {


        // 检查是否圆环
    // uint8 l_border_u8[UVC_HEIGHT];
    // uint8 r_border_u8[UVC_HEIGHT];
    
    // for(int i = 0; i < UVC_HEIGHT; i++) {
    //     l_border_u8[i] = (uint8)l_border[i];
    //     r_border_u8[i] = (uint8)r_border[i];
    // }
    
    // if(iscircle(l_border_u8, r_border_u8, bin_image)) {
    //     current_track_type = CIRCLE_TRACK;

    //     return;
    // }

    // 1. 检查是否十字路口
    if (data_stastics_l > UVC_HEIGHT/2 && data_stastics_r > UVC_HEIGHT/2) {
        uint16 i;
        uint16 left_width = 0, right_width = 0;
        
        // 计算左右边界宽度
        for (i = UVC_HEIGHT/2; i < UVC_HEIGHT-1; i++) {
            left_width += l_border[i];
            right_width += (UVC_WIDTH - r_border[i]);
        }
        
        // 如果左右边界都很宽，可能是十字
        if (left_width > UVC_WIDTH && right_width > UVC_WIDTH) {
            cross_detected = 1;
            current_track_type = CROSS_TRACK;

            return;
        }
    }
    

    // 3. 检查是否斑马线
    if(!zebra_detected) {
        // 检测图像中下部三行
        detect_zebra_line(UVC_HEIGHT - 3);
        if(!zebra_detected) detect_zebra_line(UVC_HEIGHT - 5);
        if(!zebra_detected) detect_zebra_line(UVC_HEIGHT - 7);
        
        if(zebra_detected) {
            current_track_type = ZEBRA_TRACK;
            return;
        }
    }

}



// 坡道处理
// void ramp_track_process(void) {
//     // 坡道处理逻辑
//     // 1. 保持直行
//     for (uint8 i = 0; i < UVC_HEIGHT; i++) {
//         midline[i] = UVC_WIDTH / 2;
//     }
    
//     // 2. 检查是否离开坡道
//     uint8 white_pixels = 0;
//     for (uint8 i = 0; i < UVC_HEIGHT/2; i++) {
//         for (uint8 j = UVC_WIDTH/4; j < UVC_WIDTH*3/4; j++) {
//             if (bin_image[i][j] == WHITE_PIXEL) {
//                 white_pixels++;
//             }
//         }
//     }
    
//     if (white_pixels < (UVC_HEIGHT/2)*(UVC_WIDTH/2)/4) {
//         ramp_detected = 0;
//         current_track_type = NORMAL_TRACK;
//         current_state = SEARCH_MODE;
//     }
// }

// 十字处理
void cross_track_process(void) {
    uint8 hightest = 0;
    
    // 使用八邻域算法寻找边界
    if(get_start_point(UVC_HEIGHT - 2)) {
        search_l_r((uint16)USE_NUM, bin_image, &data_stastics_l, &data_stastics_r, 
                  start_point_l[0], start_point_l[1], start_point_r[0], start_point_r[1], &hightest);
        
        // 从边界点中提取边界线
        get_left(data_stastics_l);
        get_right(data_stastics_r);

        // 调用十字补线函数
        cross_fill(bin_image, l_border, r_border, data_stastics_l, data_stastics_r, 
                 dir_l, dir_r, points_l, points_r);
        
        // 计算中线 
        for(uint8 i = hightest; i < UVC_HEIGHT-1; i++) {
            midline[i] = (l_border[i] + r_border[i]) / 2;
            leftline[i] = l_border[i];
            rightline[i] = r_border[i];
        }
        
        // 检查是否离开十字
        uint8 left_width = 0, right_width = 0;
        for (uint8 i = UVC_HEIGHT/2; i < UVC_HEIGHT-1; i++) {
            left_width += l_border[i];
            right_width += (UVC_WIDTH - r_border[i]);
        }
        
        if (left_width < UVC_WIDTH/4 || right_width < UVC_WIDTH/4) {
            cross_detected = 0;
            current_track_type = NORMAL_TRACK;
            current_state = SEARCH_MODE;
        }
    } else {
        lost_counter++;
        if (lost_counter > 5) {
            current_state = SEARCH_MODE;
        }
    }
}

// 普通赛道处理
void normal_track_process(void) {
    uint8 hightest = 0;
    
    // 使用八邻域算法寻找边界
    if(get_start_point(UVC_HEIGHT - 2)) { // 找到起点
        search_l_r((uint16)USE_NUM, bin_image, &data_stastics_l, &data_stastics_r, 
                  start_point_l[0], start_point_l[1], start_point_r[0], start_point_r[1], &hightest);
        
        // 从边界点中提取边界线
        get_left(data_stastics_l);
        get_right(data_stastics_r);
        
        // 计算中线 
        for(uint8 i = hightest; i < UVC_HEIGHT-1; i++) {
            midline[i] = (l_border[i] + r_border[i]) / 2;
            leftline[i] = l_border[i];
            rightline[i] = r_border[i];
        }
        
        // 中线计算平滑滤波
        for(int i = 1; i < UVC_HEIGHT-1; i++) {
            if(midline[i] > 0 && midline[i-1] > 0 && midline[i+1] > 0) {
                midline[i] = (midline[i-1] + midline[i] + midline[i+1]) / 3;
            }
        }
        
        lost_counter = 0; // 重置丢线计数器
        current_state = TRACKING_MODE;
    } else {
        lost_counter++;
        if (lost_counter > 5) {
            current_state = SEARCH_MODE;
        }
    }
}

// 主图像处理函数
void image_process() {
  
    cv::Mat frame, grayFrame, binaryFrame;

    // 使用OpenCV打开摄像头
    cv::VideoCapture cap("/dev/video0");
    if(!cap.isOpened()) {
        printf("Error: Could not open camera!\n");
        return;
    }

    // 设置摄像头分辨率
    cap.set(cv::CAP_PROP_FRAME_WIDTH, UVC_WIDTH);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, UVC_HEIGHT);
        // // // // 设置其他摄像头参数
    // cap.set(cv::CAP_PROP_BRIGHTNESS, 0);       // 亮度
    // cap.set(cv::CAP_PROP_CONTRAST, 32);        // 对比度
    // cap.set(cv::CAP_PROP_HUE, 0);              // 色调
    // cap.set(cv::CAP_PROP_SATURATION, 60);      // 饱和度
    // cap.set(cv::CAP_PROP_SHARPNESS, 2);        // 清晰度
    // cap.set(cv::CAP_PROP_GAMMA, 100);          // 伽马
    // cap.set(cv::CAP_PROP_WHITE_BALANCE_BLUE_U, 6500); // 白平衡
    // cap.set(cv::CAP_PROP_BACKLIGHT, 1);        // 逆光对比
    // cap.set(cv::CAP_PROP_GAIN, 0);             // 增益    
    // cap.set(cv::CAP_PROP_AUTO_EXPOSURE, 3);
    cap.set(cv::CAP_PROP_AUTO_EXPOSURE, 0);
    cap.set(cv::CAP_PROP_EXPOSURE, 156);


    // 初始化状态机
    state_machine_init();

    while(1) {
        
        // 从摄像头读取一帧
        if(!cap.read(frame)) {
            printf("Error: Could not read frame!\n");
            continue;
        }
        
        if(frame.empty()) {
            printf("Warning: Empty frame!\n");
            continue;
        }
        
        // 转换为灰度图
        cv::cvtColor(frame, grayFrame, cv::COLOR_BGR2GRAY);
        
        // 将OpenCV图像数据复制到原始图像数组
        for(int i = 0; i < UVC_HEIGHT; i++) {
            for(int j = 0; j < UVC_WIDTH; j++) {
                original_image[i][j] = grayFrame.at<uint8_t>(i, j);
            }
        }
        
        // 图像处理流程
        turn_to_bin();                  // 二值化
        image_filter(bin_image);        // 形态学滤波
        image_draw_rectan(bin_image);   // 绘制边框


        //电机闭环
        // control_motor_use();

        // 无刷调速          
        // brushless_set_speed(brushless_get_motor1(), 20);           
        // brushless_set_speed(brushless_get_motor2(), 20);
        

        MotorController_setMotors(open_speed,open_speed);

        // 清零统计变量
        data_stastics_l = 0;
        data_stastics_r = 0;
        
        // 状态机处理
        switch(current_state) {
        // switch(SPECIAL_MODE) {
            case SEARCH_MODE:
                detect_track_type();
                
                if (current_track_type != NORMAL_TRACK) {
                    current_state = SPECIAL_MODE;
                } else {
                    normal_track_process();
                }
                break;
                
            case TRACKING_MODE:
                detect_track_type();
                
                if (current_track_type != NORMAL_TRACK) {
                    current_state = SPECIAL_MODE;
                } else {
                    normal_track_process();
                }
                break;
                
            case SPECIAL_MODE:
                // 特殊元素处理
                switch(current_track_type) {
                    case CROSS_TRACK:
                        cross_track_process();
                        break;
                    case CIRCLE_TRACK:
                        circle_track_process();
                        break;
            
                    case ZEBRA_TRACK:
                            if(zebra_count == 2) 
                            {
                                normal_track_process();
                                open_speed = 0; // 停车

                            }
                        break;
                        
                    default:
                        normal_track_process();
                        break;
                }
                break;
        }
        
        // 显示图像
        uint8 l_border_u8[UVC_HEIGHT];
        uint8 r_border_u8[UVC_HEIGHT];
        for(int i = 0; i < UVC_HEIGHT; i++) {
            l_border_u8[i] = (uint8)l_border[i];
            r_border_u8[i] = (uint8)r_border[i];
        }
        ips200_show_binary_image_with_colored_lines(0, 0, bin_image[0], UVC_WIDTH, UVC_HEIGHT, midline, l_border_u8, r_border_u8);
    }

    cap.release(); // 释放摄像头
}